#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
Servo tracking logic migrated from legacy implementation with configuration support.
"""

import logging
import threading
import time
from typing import Optional, Tuple

import numpy as np

from ...config.settings import TrackingConfig
from .pid_controller import PIDController

logger = logging.getLogger("RDK_YOLO")


class ServoTracker:
    """
    舵机自动跟踪控制器。
    """

    def __init__(self, servo_controller, tracking_config: TrackingConfig):
        self.servo_controller = servo_controller
        self.tracking_config = tracking_config
        servo_cfg = tracking_config.servo_tracking

        self.tracking_thread: Optional[threading.Thread] = None
        self.auto_tracking = False
        self.coord_calculator = None

        self.pid_pan = PIDController.from_config(tracking_config.pid.pan)
        self.pid_tilt = PIDController.from_config(tracking_config.pid.tilt)

        self.face_timeout = servo_cfg.face_timeout
        self.movement_threshold = servo_cfg.movement_threshold
        self.angle_adjust_factor = servo_cfg.angle_adjust_factor
        self.min_servo_interval = servo_cfg.min_servo_interval
        self.distance_threshold = servo_cfg.distance_threshold
        self.sleep_interval = servo_cfg.sleep_interval
        self.first_detection_factor = servo_cfg.first_detection_factor
        self.max_angle_change = servo_cfg.max_angle_change
        self.center_deadzone = servo_cfg.center_deadzone

        self.prediction_enabled = servo_cfg.prediction_enabled
        self.prediction_time = servo_cfg.prediction_time
        self.prediction_weight = servo_cfg.prediction_weight
        self.max_prediction_distance = servo_cfg.max_prediction_distance
        self.angle_smoothing_factor = max(0.0, min(servo_cfg.angle_smoothing_factor, 1.0))

        self.last_servo_move_time = 0.0
        self.is_first_detection = True
        self.last_bbox: Optional[Tuple[int, int, int, int]] = None
        self.last_offset_x = 0.0
        self.last_offset_y = 0.0
        self.smoothed_pan = self.servo_controller.current_pan
        self.smoothed_tilt = self.servo_controller.current_tilt

    def start_tracking(self, coord_calculator):
        if self.auto_tracking:
            logger.info("自动跟踪已经在运行中")
            return True

        if not self.servo_controller.is_connected():
            logger.error("未连接到串口设备，无法启动自动跟踪")
            return False

        logger.info("开始自动跟踪")
        self.auto_tracking = True
        self.coord_calculator = coord_calculator

        if self.coord_calculator:
            self.coord_calculator.latest_bbox = None
            self.coord_calculator.latest_update_time = 0.0

        self.pid_pan.reset()
        self.pid_tilt.reset()
        self.is_first_detection = True
        self.last_bbox = None
        self.last_offset_x = 0.0
        self.last_offset_y = 0.0
        self.smoothed_pan = self.servo_controller.current_pan
        self.smoothed_tilt = self.servo_controller.current_tilt

        self.tracking_thread = threading.Thread(target=self._tracking_task, daemon=True)
        self.tracking_thread.start()
        return True

    def stop_tracking(self):
        if not self.auto_tracking:
            logger.info("自动跟踪未在运行")
            return True

        logger.info("停止自动跟踪")
        self.auto_tracking = False
        if self.tracking_thread and self.tracking_thread.is_alive():
            self.tracking_thread.join(timeout=1.0)
        return True

    def _tracking_task(self):
        logger.info("自动跟踪线程已启动")
        last_update_time = time.time()

        try:
            while self.auto_tracking:
                current_time = time.time()

                if self.coord_calculator and self.coord_calculator.latest_bbox is not None:
                    bbox_timestamp = self.coord_calculator.latest_update_time

                    if current_time - bbox_timestamp < self.face_timeout:
                        time_diff = current_time - last_update_time
                        last_update_time = current_time

                        bbox = self.coord_calculator.latest_bbox
                        self.last_bbox = bbox

                        offset_x, offset_y, distance = self.coord_calculator.calculate_offset(bbox)
                        self.last_offset_x = offset_x
                        self.last_offset_y = offset_y

                        if (self.prediction_enabled and
                                self.coord_calculator.current_target_id is not None and
                                self.coord_calculator.current_target_id in self.coord_calculator.target_history):
                            target_id = self.coord_calculator.current_target_id
                            target_info = self.coord_calculator.target_history[target_id]

                            if "velocity" in target_info and "velocity_magnitude" in target_info:
                                velocity = target_info["velocity"]
                                velocity_magnitude = target_info["velocity_magnitude"]

                                if velocity_magnitude > 10:
                                    prediction_x = offset_x + velocity[0] * self.prediction_time
                                    prediction_y = offset_y + velocity[1] * self.prediction_time

                                    prediction_distance = np.hypot(prediction_x - offset_x, prediction_y - offset_y)
                                    if prediction_distance > self.max_prediction_distance:
                                        scale = self.max_prediction_distance / prediction_distance
                                        prediction_x = offset_x + (prediction_x - offset_x) * scale
                                        prediction_y = offset_y + (prediction_y - offset_y) * scale

                                    dynamic_weight = min(self.prediction_weight * velocity_magnitude / 50, 0.8)
                                    offset_x = offset_x * (1 - dynamic_weight) + prediction_x * dynamic_weight
                                    offset_y = offset_y * (1 - dynamic_weight) + prediction_y * dynamic_weight

                        if distance < self.center_deadzone:
                            logger.debug("目标在中心死区内(距离=%.1f像素)，暂停追踪", distance)
                            time.sleep(self.sleep_interval)
                            continue

                        if abs(offset_x) < self.movement_threshold and abs(offset_y) < self.movement_threshold:
                            time.sleep(self.sleep_interval)
                            continue

                        if current_time - self.last_servo_move_time < self.min_servo_interval:
                            time.sleep(self.sleep_interval)
                            continue

                        pan_delta = self.pid_pan.update(offset_x, time_diff)
                        tilt_delta = self.pid_tilt.update(offset_y, time_diff)

                        adjust_factor = self.angle_adjust_factor
                        if self.is_first_detection:
                            adjust_factor *= self.first_detection_factor
                            self.is_first_detection = False

                        pan_delta_angle = pan_delta * adjust_factor
                        tilt_delta_angle = tilt_delta * adjust_factor

                        pan_delta_angle = max(-self.max_angle_change, min(self.max_angle_change, pan_delta_angle))
                        tilt_delta_angle = max(-self.max_angle_change, min(self.max_angle_change, tilt_delta_angle))

                        pan_angle = self.servo_controller.current_pan - pan_delta_angle
                        tilt_angle = self.servo_controller.current_tilt - tilt_delta_angle

                        pan_angle = self._apply_smoothing(self.smoothed_pan, pan_angle)
                        tilt_angle = self._apply_smoothing(self.smoothed_tilt, tilt_angle)
                        self.smoothed_pan = pan_angle
                        self.smoothed_tilt = tilt_angle

                        if current_time - self.last_servo_move_time >= self.min_servo_interval:
                            servo_angles = {
                                self.servo_controller.gimbal_config.pan_pin: pan_angle,
                                self.servo_controller.gimbal_config.tilt_pin: tilt_angle,
                            }
                            if self.servo_controller.move_servos_together(servo_angles):
                                self.last_servo_move_time = current_time
                            else:
                                logger.warning("舵机移动失败，暂停本次调整")

                    else:
                        time.sleep(self.sleep_interval)
                else:
                    time.sleep(self.sleep_interval)

        except Exception as e:
            logger.error("自动跟踪线程错误: %s", e, exc_info=True)
        finally:
            logger.info("自动跟踪线程结束")

    def _apply_smoothing(self, previous: float, target: float) -> float:
        if self.angle_smoothing_factor <= 0.0:
            return target
        if self.angle_smoothing_factor >= 1.0:
            return target
        return previous + self.angle_smoothing_factor * (target - previous)
